/*
 * mpcc.c
 *
 *  Created on: Apr 1, 2024
 *      Author: Yiwen Zhu
 */
#include "mpcc.h"
#include "math.h"
#include "string.h"
#include "Motor_Control_Fcns.h"
#include "Motor_Control_Init.h"

#define HRTIM_ZERO_CMP	(0)
#define HRTIM_FULL_CMP	(DEF_SVM_TO_CMP)

//#define VAL1  	0.0f
//#define VAL2  	DRV_UDC*(0.5443f)
//#define VAL3	DRV_UDC*(0.2722f)
//#define VAL4	DRV_UDC*(-0.2722f)
//#define VAL5	DRV_UDC*(-0.5443f)
//#define VAL6	DRV_UDC*(-0.2722f)
//#define VAL7	DRV_UDC*(0.2722f)
//
//#define VBE1  	0.0f
//#define VBE2  	0.0f
//#define VBE3	DRV_UDC*(0.4714f)
//#define VBE4	DRV_UDC*(0.4714f)
//#define VBE5	0.0f
//#define VBE6	DRV_UDC*(-0.4714f)
//#define VBE7	DRV_UDC*(-0.4714f)
//
//const float Val[7] = {VAL1,VAL2,VAL3,VAL4,VAL5,VAL6,VAL7};
//const float Vbe[7] = {VBE1,VBE2,VBE3,VBE4,VBE5,VBE6,VBE7};

static const char State[7][3] = {
		{0, 0, 0},	// State V0/V7
		{1, 0, 0},
		{1, 1, 0},
		{0, 1, 0},
		{0, 1, 1},
		{0, 0, 1},
		{1, 0, 1},
};

void mpcc_param_update_online(void){
	if(DEF_CTL_STATE==RUN){
	 }
}

void mpcc_init(void){
	mpcc1.Udc = AD_BUF[2]*DEF_AD_TO_REAL_VDC;

	mpcc1.Val[0] = 0.0f;
	mpcc1.Val[1] = mpcc1.Udc*0.5443f;
	mpcc1.Val[2] = mpcc1.Udc*0.2722f;
	mpcc1.Val[3] = mpcc1.Udc*-0.2722f;
	mpcc1.Val[4] = mpcc1.Udc*-0.5443f;
	mpcc1.Val[5] = mpcc1.Udc*-0.2722f;
	mpcc1.Val[6] = mpcc1.Udc*0.2722f;

	mpcc1.Vbe[0] = 0.0f;
	mpcc1.Vbe[1] = 0.0f;
	mpcc1.Vbe[2] = mpcc1.Udc*0.4714f;
	mpcc1.Vbe[3] = mpcc1.Udc*0.4714f;
	mpcc1.Vbe[4] = 0.0f;
	mpcc1.Vbe[5] = mpcc1.Udc*-0.4714f;
	mpcc1.Vbe[6] = mpcc1.Udc*-0.4714f;


	mpcc1.K1 = mpcc1.Ts /mpcc1.Ld;
	mpcc1.K2 = mpcc1.Rs /mpcc1.Ld *mpcc1.Ts;
	mpcc1.K3 = mpcc1.Lq /mpcc1.Ld *mpcc1.Ts;

	mpcc1.K4 = mpcc1.Ts /mpcc1.Lq;
	mpcc1.K5 = mpcc1.Rs /mpcc1.Lq *mpcc1.Ts;
	mpcc1.K6 = mpcc1.Ld /mpcc1.Lq *mpcc1.Ts;
	mpcc1.K7 = mpcc1.Psif /mpcc1.Lq *mpcc1.Ts;
}


void mpcc_task(void){
	static int Ind_Last = 0;
//	static int Loop_Index = 0;

	float id_kp1,iq_kp1,id_kp2,iq_kp2;
	float temp1,temp2;
	float Vd,Vq;
	int Ind=0;
	float g;
	float gmin = 1000.0f; // Must large enough!

	// One-step delay compensation
	id_kp1 = mpcc1.Fdb_Id - mpcc1.K2*mpcc1.Fdb_Id  + mpcc1.K1*mpcc1.Vd_Last + mpcc1.K3*mpcc1.Fdb_Iq*mpcc1.We; //
	iq_kp1 = mpcc1.Fdb_Iq - mpcc1.K5*mpcc1.Fdb_Iq  + mpcc1.K4*mpcc1.Vq_Last + mpcc1.K6*mpcc1.Fdb_Id*mpcc1.We -mpcc1.K7*mpcc1.We ; //

	// Pre-calculate the common part
	temp1 = id_kp1 - mpcc1.K2*id_kp1 + mpcc1.K3*iq_kp1*mpcc1.We;
	temp2 = iq_kp1 - mpcc1.K5*iq_kp1 -mpcc1.K7*mpcc1.We +mpcc1.K6*id_kp1*mpcc1.We;

	// Search the "optimal" voltage vector
	for(int i=0;i<7;i++){
		Vd = mpcc1.cos*mpcc1.Val[i] +mpcc1.sin*mpcc1.Vbe[i];
		Vq = -mpcc1.sin*mpcc1.Val[i] +mpcc1.cos*mpcc1.Vbe[i];
		id_kp2 = temp1 + mpcc1.K1*Vd ;	 //
		iq_kp2 = temp2 + mpcc1.K4*Vq ; //

		g = fabsf(mpcc1.Ref_Id -id_kp2)+ fabsf(mpcc1.Ref_Iq -iq_kp2) ;

		if(g<=gmin){
			gmin = g;
			Ind = i;
			mpcc1.Vd_Last = Vd;
			mpcc1.Vq_Last = Vq;
		}
	}

//	if(Loop_Index++>=2)	Loop_Index = 0;
//	Ind = Loop_Index;

	// If the switching state is 1，set a higher compare value
	if(State[Ind][0]==1)	mpcc1.Ta = HRTIM_FULL_CMP;
	else					mpcc1.Ta = HRTIM_ZERO_CMP;

	if(State[Ind][1]==1)	mpcc1.Tb = HRTIM_FULL_CMP;
	else					mpcc1.Tb = HRTIM_ZERO_CMP;

	if(State[Ind][2]==1)	mpcc1.Tc = HRTIM_FULL_CMP;
	else					mpcc1.Tc = HRTIM_ZERO_CMP;

	// record the last period value
	mpcc1.Index_Last2 = mpcc1.Index_Last;
	mpcc1.Index_Last = Ind_Last;
	Ind_Last = Ind;
	mpcc1.g_opt = gmin;

}


